#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <tf/transform_listener.h>
#include <fstream>

#include <ros/package.h>
#include <string>
#include <regex>
#include <filesystem>
#include <stdexcept>

class PcdSaver
{
public:
    PcdSaver(ros::NodeHandle &nh) : nh_(nh)
    {
        // 读取参数
        nh_.param<std::string>("cloud_topic_name", cloud_topic_name_, "/cloud_registered");
        nh_.param<std::string>("target_frame_id", target_frame_id_, "base_link");
        nh_.param<std::string>("world_frame_id", world_frame_id_, "map");
        nh_.param<double>("save_interval", save_interval_, 60.0);
        nh_.param<std::string>("save_config_path", save_config_path_, "$(find map_trans_toolkit_pkg)/config/map_serial.yaml");
        nh_.param<std::string>("save_dir", save_dir_, "$(find map_trans_toolkit_pkg)/maps/map_serial");

        // 订阅点云话题
        cloud_sub_ = nh_.subscribe(cloud_topic_name_, 1, &PcdSaver::cloudCallback, this);

        // 初始化点云
        combined_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());

        // 解析路径
        std::string package_path = ros::package::getPath("map_trans_toolkit_pkg");
        save_config_path_ = resolvePath(save_config_path_, "map_trans_toolkit_pkg");
        save_dir_ = resolvePath(save_dir_, "map_trans_toolkit_pkg");

        // 检查日志文件和目录是否存在，如果不存在则创建，如果存在则删除后再创建
        boost::filesystem::path log_dir(save_config_path_.c_str());
        log_dir = log_dir.parent_path();
        if (boost::filesystem::exists(log_dir))
        {
            boost::filesystem::remove_all(log_dir);
        }
        boost::filesystem::create_directories(log_dir);

        boost::filesystem::path map_dir(save_dir_.c_str());
        if (boost::filesystem::exists(map_dir))
        {
            boost::filesystem::remove_all(map_dir);
        }
        boost::filesystem::create_directories(map_dir);
    }

    void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
    {
        // 转换为 PCL 点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(*cloud_msg, *cloud);

        // 拼接点云
        *combined_cloud_ += *cloud;

        // 检查是否到达保存时间
        if ((ros::Time::now() - start_time_).toSec() >= save_interval_)
        {
            savePcdFile();
            reset();
        }
    }

private:
    // 定义替换函数
    std::string resolvePath(const std::string &path, const std::string &package_name)
    {
        // 获取包路径
        std::string package_path = ros::package::getPath(package_name);
        if (package_path.empty())
        {
            throw std::runtime_error("Package path not found for: " + package_name);
        }

        // 替换路径中的 $(find package_name) 占位符
        std::string result = path;
        boost::replace_all(result, "$(find " + package_name + ")", package_path);

        // 使用 C++17 的 filesystem 规范化路径（处理冗余的 ./, ../ 等）
        std::filesystem::path canonical_path = std::filesystem::weakly_canonical(result);

        return canonical_path.string();
    }

    void savePcdFile()
    {
        // 生成文件名
        std::string filename = save_dir_ + "/cloud_" + std::to_string(save_count_++) + ".pcd";

        // 保存点云
        pcl::io::savePCDFileASCII(filename, *combined_cloud_);

        // 获取 TF 变换
        tf::StampedTransform transform;
        try
        {
            tf_listener_.lookupTransform(world_frame_id_, target_frame_id_, ros::Time(0), transform);
        }
        catch (tf::TransformException &ex)
        {
            ROS_WARN("%s", ex.what());
        }

        // 记录配置信息
        std::ofstream ofs(save_config_path_, std::ios::app);
        if (ofs.is_open())
        {
            ofs << filename << "," << ros::Time::now().toSec() << "," << transform.getOrigin().x() << ","
                << transform.getOrigin().y() << "," << transform.getOrigin().z() << ","
                << transform.getRotation().x() << "," << transform.getRotation().y() << ","
                << transform.getRotation().z() << "," << transform.getRotation().w() << std::endl;
        }
    }

    void reset()
    {
        combined_cloud_->clear();
        start_time_ = ros::Time::now();
    }

    ros::NodeHandle nh_;
    ros::Subscriber cloud_sub_;

    std::string cloud_topic_name_;
    std::string target_frame_id_;
    std::string world_frame_id_;
    double save_interval_;
    std::string save_config_path_;
    std::string save_dir_;

    int save_count_ = 0;
    pcl::PointCloud<pcl::PointXYZ>::Ptr combined_cloud_;
    ros::Time start_time_;
    tf::TransformListener tf_listener_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pcd_saver_node");
    ros::NodeHandle nh;

    PcdSaver pcd_saver(nh);
    ros::spin();

    return 0;
}

/*
pcd_saver_node
读取参数有
1. cloud_topic_name
2. target_frame_id
3. world_frame_id
4. save_interval
5. save_config_path
6. save_dir

功能为订阅指定话题下的点云数据，并进行拼接。
时间到达 save_interval (单位秒)后保存从开始到此时的pcd, 然后清空，重新开始拼接。
每次保存时都会记录下 [ 保存的文件名, 保存的时间, 开始记录时world_frame到target_frame 的 tf ]，追加到 save_config_path 文件中。
save_config_path 下的文件是一个 csv 文件，每一行代表一个 pcd 文件，每一列代表一个信息。

初始化时注意对于路径的解析（形如$(find map_pkg)/map ）
如果没有指定目录或者文件，则新建。
*/